import numpy as np
import numpy.linalg as nm
import partie2 as p2
from math import sqrt
import partie1 as p1


def diagonale_convergence(M,Nmax):
    n=M.shape[0]
    U=np.eye(n,n)
    V=np.eye(n,n)
    (Ql,S,Qr)=p2.bidiagonal_transformation(M)
    
    for i in range(Nmax):
        (Q1,R1)=nm.qr(np.transpose(S))
        (Q2,R2)=nm.qr(np.transpose(R1))
        S=R2
        U=np.dot(U,Q2)
        V=np.dot(np.transpose(Q1),V)
        
        
    return (np.dot(Ql,U),S,np.dot(V,Qr))
    

def max(X):
    X=np.asarray(X)
    X.shape=X.size
    indice=0
    for i in range(X.size):
        if(X[indice]<X[i]):
            indice=i
    return indice
        
        
def diagonale_convergence_optimized(M,Nmax):
    n=M.shape[0]
    U=np.eye(n,n)
    V=np.eye(n,n)
    (Ql,S,Qr)=p2.bidiagonal_transformation(M)
    
    for i in range(Nmax):
        (Q1,R1)=qr_optimized(np.transpose(S))
        (Q2,R2)=qr_optimized(np.transpose(R1))


        S=R2
        U=np.dot(U,Q2)
        V=np.dot(np.transpose(Q1),V)
    #signe=np.eye(n,n)
    
    #for i in range(n):
     #   if(S[i,i]<=0):
      #      signe[i,i]=-1
    #print signe
    #S=np.dot(S,signe)
    #V=np.dot(signe,V)
    #for j in range (n-1):
        
     #   L=S[j:n,j:n]
      #  X=np.diag(L)
       
       # i=max(X)+j
        
       # if(i!=j):
        #    P=np.eye(n,n)
        #    P[i,i]=0
        #    P[j,j]=0
         #    P[i,j]=1
         #   P[j,i]=1
         #   Y=S[:,i]
         #   X=S[:,j]
         #   S[:,j]=Y
         #   S[:,i]=X
            
            
            
   # V=np.dot(P,V)

            
    
    
    return (np.dot(Ql,U),S,np.dot(V,Qr))
     
def qr_optimized(M):
    n=M.shape[0]
    Q=np.eye(n,n)
    for i in range(n-1):
        (Q,M)=householder_optimized(i,Q,M)
        
    return (Q,M)


def householder_optimized(i,Q,M):
    U=np.array([1.,1.])
    U.shape=(2)
    norme=sqrt(M[i,i]**2+M[i+1,i]**2)
    
    U[0]=M[i,i]-norme
    U[1]=M[i+1,i]
    norme=sqrt(U[0]**2+U[1]**2)
    U[0]=U[0]/norme
    U[1]=U[1]/norme
    
    scalar1=U[0]*M[i,i]+U[1]*M[i+1,i]
    M[i,i]=M[i,i]-2*scalar1*U[0]
    M[i+1,i]=M[i+1,i]-2*scalar1*U[1]
    scalar2=U[1]*M[i+1,i+1]
    
    M[i,i+1]=M[i,i+1]-2*scalar2*U[0]
    M[i+1,i+1]=M[i+1,i+1]-2*scalar2*U[1]
    for j in range(i+1):
        
        
        Q[j,i+1]=Q[j,i]*(-2*U[0]*U[1])
        Q[j,i]=Q[j,i]*(1-2*(U[0]**2))
        
    

        Q[i+1,i]=-2*U[0]*U[1]
        Q[i+1,i+1]=1-2*(U[1]**2)

    return (Q,M)


N=np.matrix([[0,7,12,34],[2,91,0,5],[3,42,2,32],[-45,-7,-8,-9]])
diagonale_convergence_optimized(np.transpose(N),20)
